#include "solve.h"
#include "matrix.h"

double* jacobi(Matrix a, double* p)         //jacobi迭代法
{
    double eps = 1e-7;
    Matrix c(a);
    Matrix b(a.row, 1, 0);
    Matrix d(a.row, 1,p);
    for (int i = 0; i < a.row; i++)
    {
        b.datap[i] = 1/a.datap[i * (a.col + 1)];
        a.datap[i * (a.col + 1)] = 0;
    }
    for (int i = 0; i < a.col; i++)
    {
        for (int j = 0; j < a.col; j++)
        {
            a.datap[i * a.col + j] *= b.datap[i];
        }
        b.datap[i] = b.datap[i] * p[i];
    }
    Matrix x(a.row, 1, 2);
    while (vnorm(c*x - d)>eps)
    {
        x = b - a * x ;
    }
    x.show();
}

double* GS(Matrix a, double* p)         //高斯迭代法
{
    double eps = 1e-7;
    Matrix c = a.down();
    Matrix d(a.row, 1,p);
    Matrix e = c.downarc();
    Matrix f = e * (c-a);
    Matrix g = e * d;
    Matrix x(a.row, 1, 2);
    while (vnorm(a*x - d)>eps)
    {
        x = f * x + g ;
    }
    x.show();
}

double* SOR(Matrix a, double* p, double w)          //松弛迭代法，w为松弛因子
{
    double eps = 1e-7;
    Matrix c = a.down();
    Matrix b = a.diag();
    Matrix d(a.row, 1,p);
    Matrix e = diag(a.row, b.datap);
    Matrix f = e + (c-e) * w;
    Matrix g = f.downarc();
    Matrix i = e * (1 - w) + (c-a) * w;
    Matrix k = g * i;
    Matrix l = g * d;
    Matrix x(a.row, 1, 2);
    while (vnorm(a*x - d)>eps)
    {
        x = k * x + l*w ;
    }
    x.show();
}

/*int main()
{
    double p[9] = {4, 1, 0, 1, 4, 1, 0, 1, 4};
    Matrix a(3, 3, p);
    double b[3] = {14, 12, 6};
    jacobi(a, b);
    GS(a, b);
    SOR(a, b, 1.5);
    return 0;
}*/